﻿using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using ARCam3D.Tuple;
using ARCam3D.Transform;

namespace ARCam3D.Imaging.Processing
{
    public class MarkerDetector
    {

        private ComponentsDetector componentsDetector = new ComponentsDetector();
        private VectorImageBinarizator vectorImageBinarizator = new VectorImageBinarizator();
        private CornersDetector cornersDetector = new CornersDetector();
        private EdgeDetector edgeDetector = new EdgeDetector();
        private ContourChecker contourChecker = new ContourChecker();
        private GrayscaleImageConverter grayscaleImageConverter = new GrayscaleImageConverter();
        //private SampleImageChecker sampleImageChecker;

        private CameraProjectionTransform projectionTransform;
        private double markerSideLength;

        public MarkerDetector(CameraProjectionTransform projectionTransform, double markerSideLength)
        {
            this.projectionTransform = projectionTransform;
            this.markerSideLength = markerSideLength;
            //sampleImageChecker = new SampleImageChecker(new Bitmap("marker.bmp"), markerSideLength, projectionTransform);
        }

        public MatrixTransform3D GetMarkerTransform(RGBImage image, Point2I[] projectionPointsPx)
        {
            Point2D[] projectionPoints = new Point2D[4];

            projectionPoints[0] = projectionTransform.GetWorldPoint(projectionPointsPx[0]);
            projectionPoints[1] = projectionTransform.GetWorldPoint(projectionPointsPx[1]);
            projectionPoints[2] = projectionTransform.GetWorldPoint(projectionPointsPx[2]);
            projectionPoints[3] = projectionTransform.GetWorldPoint(projectionPointsPx[3]);

            Vector3D[] normals = new Vector3D[4];

            normals[0] = new Vector3D(
                (projectionPoints[0].y - projectionPoints[1].y) * projectionTransform.ProjectionPlaneDistance,
                (projectionPoints[1].x - projectionPoints[0].x) * projectionTransform.ProjectionPlaneDistance,
                projectionPoints[0].x * projectionPoints[1].y - projectionPoints[1].x * projectionPoints[0].y);
            normals[1] = new Vector3D(
                (projectionPoints[2].y - projectionPoints[3].y) * projectionTransform.ProjectionPlaneDistance,
                (projectionPoints[3].x - projectionPoints[2].x) * projectionTransform.ProjectionPlaneDistance,
                projectionPoints[2].x * projectionPoints[3].y - projectionPoints[3].x * projectionPoints[2].y);

            normals[2] = new Vector3D(
                (projectionPoints[1].y - projectionPoints[2].y) * projectionTransform.ProjectionPlaneDistance,
                (projectionPoints[2].x - projectionPoints[1].x) * projectionTransform.ProjectionPlaneDistance,
                projectionPoints[1].x * projectionPoints[2].y - projectionPoints[2].x * projectionPoints[1].y);
            normals[3] = new Vector3D(
                (projectionPoints[3].y - projectionPoints[0].y) * projectionTransform.ProjectionPlaneDistance,
                (projectionPoints[0].x - projectionPoints[3].x) * projectionTransform.ProjectionPlaneDistance,
                projectionPoints[3].x * projectionPoints[0].y - projectionPoints[0].x * projectionPoints[3].y);

            Vector3D markerXVector = Vector3D.VectorMul(normals[1], normals[0]).Normalize();
            Vector3D markerYVector = Vector3D.VectorMul(normals[3], normals[2]).Normalize();
            Vector3D markerZVector = Vector3D.VectorMul(markerXVector, markerYVector).Normalize();

            double k1 = markerSideLength * (markerXVector.y * projectionPoints[1].x - markerXVector.x * projectionPoints[1].y) /
                (projectionPoints[0].x * projectionPoints[1].y - projectionPoints[1].x * projectionPoints[0].y);

            double k2 = markerSideLength * (markerYVector.y * projectionPoints[3].x - markerYVector.x * projectionPoints[3].y) /
                (projectionPoints[0].x * projectionPoints[3].y - projectionPoints[3].x * projectionPoints[0].y);

            double k = (k1 + k2) / 2;

            Point3D markerShift = new Point3D(
                k * projectionPoints[0].x,
                k * projectionPoints[0].y,
                k * projectionTransform.ProjectionPlaneDistance);

            /*Point3D markerShift = new Point3D(
                - v0.x * projectionPoints[0].x - v1.x * projectionPoints[0].y - v2.x * projectionTransform.ProjectionPlaneDistance,
                - v0.y * projectionPoints[0].x - v1.y * projectionPoints[0].y - v2.y * projectionTransform.ProjectionPlaneDistance,
                - v0.z * projectionPoints[0].x - v1.z * projectionPoints[0].y - v2.z * projectionTransform.ProjectionPlaneDistance);
            */
            MatrixTransform3D result = new MatrixTransform3D(new double[,] {
                {markerXVector.x, markerYVector.x, markerZVector.x, markerShift.x},
                {markerXVector.y, markerYVector.y, markerZVector.y, markerShift.y},
                {markerXVector.z, markerYVector.z, markerZVector.z, markerShift.z}
            });


            //bool q = sampleImageChecker.CheckSample(image, result, projectionPointsPx);

            return result;



            /*return new MatrixTransform3D(new double[,] {
                {1, 0, 0, 0},
                {0, 1, 0, 0},
                {0, 0, -1, 0.5}
            });*/
        }

        public Point2I[] FindMarkerCorners(RGBImage image)
        {
            GrayscaleImage grayscaleImage = grayscaleImageConverter.Convert(image);
            
            VectorImage vectorImage = edgeDetector.DetectEdges(grayscaleImage);
            
            BinaryImage binaryImage = vectorImageBinarizator.Binarizate(vectorImage);

            /*for (int x = 0; x < binaryImage.Width; ++x)
                for (int y = 0; y < binaryImage.Height; ++y)
                    image.SetColor(x, y, binaryImage.GetValue(x, y) ? RGBColor.Black : RGBColor.White);*/

            

            List<List<Point2I>> components = componentsDetector.DetectComponents(binaryImage);

            /*RGBColor[] ccolors = new RGBColor[] { RGBColor.Red, RGBColor.Green, RGBColor.Blue, RGBColor.Cyan, RGBColor.Magenta, RGBColor.Yellow };
            for (int i = 0; i < components.Count; ++i)
                foreach (Point2I point in components[i])
                    image.SetColor(point.x, point.y, ccolors[i % ccolors.Length]);*/

            components.Sort(delegate(List<Point2I> a, List<Point2I> b) { return b.Count.CompareTo(a.Count); });
            
            Point2I[] markerCorners = null;

            foreach (List<Point2I> component in components)
            {
                Point2I[] corners = cornersDetector.DetectCorners(component, vectorImage);

                if (corners == null)
                {
                    /*foreach (Point2I point in component)
                        image.SetColor(point.x, point.y, RGBColor.Green);*/
                    continue;
                }

                if (!contourChecker.CheckContour(component, corners))
                {
                    /*foreach (Point2I point in component)
                        image.SetColor(point.x, point.y, RGBColor.Red);
                    foreach (Point2I point in corners)
                        for (int i = -1; i <= 1; ++i)
                            for (int j = -1; j <= 1; ++j)
                                image.SetColor(point.x + i, point.y + j, RGBColor.Green);*/
                    continue;
                }

                markerCorners = corners;

                /*foreach (Point2I point in component)
                    image.SetColor(point.x, point.y, RGBColor.Red);*/

                break;
            }

            if (markerCorners == null)
                return null;

            

            RGBColor[] colors = new RGBColor[] { RGBColor.Green, RGBColor.Blue, RGBColor.Yellow, RGBColor.Cyan };
            for (int q = 0; q < markerCorners.Length; ++q)
                for (int i = -1; i <= 1; ++i)
                    for (int j = -1; j <= 1; ++j)
                        image.SetColor(markerCorners[q].x + i, markerCorners[q].y + j, colors[q]);

            return markerCorners;
        }
    }
}
